package tech.runchen.gz5055.oms.common.utils;

/**
 * @author zzj
 */
public class RobotUtils {

    /**
     * 心跳区间大于五分钟判定为离线（单位秒）
     */
    private static long OFFLINE_TIME = 300;

    /**
     * 获取机器离线时间
     *
     * @return
     */
    public static long getOfflineTime() {
        return System.currentTimeMillis() / 1000 - OFFLINE_TIME;
    }

    /**
     * 计算机器到目标点的预计剩余时间
     *
     * @param position
     * @param destination
     * @return
     */
    public static double getDuration(double[] position, double[] destination) {
        // 机器人位置坐标
        double r_x = position[0];
        double r_y = position[1];
        double r_yaw = position[2];
        // 目的地位置坐标
        double n_x = destination[0];
        double n_y = destination[1];
        double n_yaw = destination[2];

        double dist_to_target = disp(new double[]{r_x, r_y},
                new double[]{n_x, n_y});
        double ori_delta = Math.abs(Math.abs(r_yaw) - Math.abs(n_yaw));
        if (ori_delta > Math.PI) {
            ori_delta = ori_delta - (2 * Math.PI);
        }
        if (ori_delta < -Math.PI) {
            ori_delta = (2 * Math.PI) + ori_delta;
        }
        double duration = (dist_to_target / 0.6 + ori_delta / 0.6);
        return duration;
    }

    /**
     * 这个不知道有什么用
     *
     * @param A
     * @param B
     * @return
     */
    private static double disp(double[] A, double[] B) {
        return Math.sqrt(Math.pow((A[0] - B[0]), 2) + Math.pow((A[1] - B[1]), 2));
    }

    /**
     * 转换思岚底盘地图的坐标点
     *
     * @param position 机器当前位置
     * @param area     地图区域
     * @return
     */
    public static double[] transformSlamtec(double[] position, double[] area, boolean isReverse) {
        double[] pose = new double[3];
        if (isReverse) {
            pose[0] = position[0] + area[0];
            pose[1] = position[1] - (area[3] / 2) - area[1];
        } else {
            pose[0] = position[0] - area[0];
            pose[1] = position[1] + area[1] + (area[3] / 2);
        }
        pose[2] = position[2];
        return pose;
    }

    /**
     * 转换SLAM地图的坐标点 TODO 未实现
     *
     * @param position 机器当前位置
     * @param origin   地图的0，0点位
     * @return
     */
    public static double[] transformSlamPosition(double[] position, double[] origin, boolean isReverse) {
        return position;
    }

    /**
     * theta与四元数转换关系
     * 四元数： {x: 0, y:0, z: -0.430000007152557, w:-0.899999976158142}
     *
     * @param z 四元数中的z
     * @param w 四元数中的w
     * @return
     */
    public double theta(double z, double w) {
        double theta = 2 * Math.atan2(z, w);
        if (theta > Math.PI && theta <= (2 * Math.PI)) {
            theta -= 2 * Math.PI;
        } else if (theta < (-1 * Math.PI) && theta >= (-2 * Math.PI)) {
            theta += 2 * Math.PI;
        }
        return theta;
    }

}
